#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from rclpy.action import ActionClient
from geometry_msgs.msg import PoseStamped
from std_msgs.msg import Float64
from nav2_msgs.action import NavigateToPose
import math


class ManualFlightControl(Node):
    """手动飞行控制节点，用于调试和测试"""
    
    def __init__(self):
        super().__init__('manual_flight_control')
        
        # 创建导航action客户端
        self.nav_action_client = ActionClient(self, NavigateToPose, '/red_standard_robot1/navigate_to_pose')
        
        # 创建高度控制发布者
        self.altitude_pub = self.create_publisher(Float64, '/target_altitude', 10)
        
        # 订阅当前位置
        self.pose_sub = self.create_subscription(
            PoseStamped,
            '/drone_pose',
            self.pose_callback,
            10
        )
        
        self.current_pose = None
        self.get_logger().info('手动飞行控制节点已启动')
        self.print_help()
        
    def pose_callback(self, msg):
        """更新当前位置"""
        self.current_pose = msg
        
    def print_help(self):
        """打印帮助信息"""
        print("\n" + "="*50)
        print("手动飞行控制命令:")
        print("="*50)
        print("高度控制:")
        print("  h <高度>     - 设置目标高度 (例: h 1.5)")
        print("导航控制:")
        print("  g <x> <y>    - 飞到指定位置 (例: g 2.0 1.0)")
        print("  home         - 返回原点 (0, 0)")
        print("状态查询:")
        print("  pos          - 显示当前位置")
        print("  help         - 显示此帮助")
        print("  quit         - 退出程序")
        print("="*50)
        
    def set_altitude(self, altitude):
        """设置目标高度"""
        altitude_msg = Float64()
        altitude_msg.data = float(altitude)
        self.altitude_pub.publish(altitude_msg)
        self.get_logger().info(f'设置目标高度: {altitude}m')
        
    def send_navigation_goal(self, x, y, yaw=0.0):
        """发送导航目标"""
        goal_msg = NavigateToPose.Goal()
        
        # 设置目标位置
        goal_msg.pose.header.frame_id = 'map'
        goal_msg.pose.header.stamp = self.get_clock().now().to_msg()
        goal_msg.pose.pose.position.x = float(x)
        goal_msg.pose.pose.position.y = float(y)
        goal_msg.pose.pose.position.z = 0.0
        
        # 设置朝向
        goal_msg.pose.pose.orientation.z = math.sin(yaw / 2.0)
        goal_msg.pose.pose.orientation.w = math.cos(yaw / 2.0)
        
        self.get_logger().info(f'发送导航目标: ({x}, {y})')
        self.nav_action_client.send_goal_async(goal_msg)
        
    def show_position(self):
        """显示当前位置"""
        if self.current_pose:
            pos = self.current_pose.pose.position
            self.get_logger().info(
                f'当前位置: x={pos.x:.3f}, y={pos.y:.3f}, z={pos.z:.3f}'
            )
        else:
            self.get_logger().warn('未收到位置信息')
            
    def run_interactive(self):
        """运行交互式控制"""
        while rclpy.ok():
            try:
                command = input("\n请输入命令 (help获取帮助): ").strip().lower()
                
                if not command:
                    continue
                    
                parts = command.split()
                cmd = parts[0]
                
                if cmd == 'quit' or cmd == 'q':
                    break
                elif cmd == 'help':
                    self.print_help()
                elif cmd == 'pos':
                    self.show_position()
                elif cmd == 'h' and len(parts) == 2:
                    try:
                        altitude = float(parts[1])
                        if 0 <= altitude <= 10:
                            self.set_altitude(altitude)
                        else:
                            print("高度必须在0-10米范围内")
                    except ValueError:
                        print("请输入有效的高度值")
                elif cmd == 'g' and len(parts) == 3:
                    try:
                        x = float(parts[1])
                        y = float(parts[2])
                        self.send_navigation_goal(x, y)
                    except ValueError:
                        print("请输入有效的坐标值")
                elif cmd == 'home':
                    self.send_navigation_goal(0.0, 0.0)
                else:
                    print("未知命令，输入 'help' 获取帮助")
                    
            except KeyboardInterrupt:
                break
            except EOFError:
                break
                
        self.get_logger().info('退出手动控制')


def main(args=None):
    rclpy.init(args=args)
    
    control_node = ManualFlightControl()
    
    # 在单独线程中运行交互式控制
    import threading
    control_thread = threading.Thread(target=control_node.run_interactive)
    control_thread.daemon = True
    control_thread.start()
    
    try:
        rclpy.spin(control_node)
    except KeyboardInterrupt:
        pass
    finally:
        control_node.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main() 